

#include "3DType.h"
#include "3DMath.h"
#include <cmath>


Float3D Vector3D::Length()const
{
	return sqrt( At(0,0)*At(0,0) + At(1,0)*At(1,0) + At(2,0)*At(2,0) );
}

Vector3D Vector3D::Unit()const
{
	Float3D length = Length();
	if ( is_zero(length) ) return Vector3D();
	else return (*this) / length;
}

Float3D Vector2D::Length()const
{
	return sqrt( At(0,0)*At(0,0) + At(1,0)*At(1,0) );
}

Vector2D Vector2D::Unit()const
{	
	Float3D length = Length();
	if ( is_zero(length) ) return Vector2D();
	else return (*this) / length;
}

Float3D Point3D::Length()const
{
	return sqrt( At(0,0)*At(0,0) + At(1,0)*At(1,0) + At(2,0)*At(2,0) );
}

Point3D Point3D::Normalize()const
{
	assert( !is_zero((*this)[3]) );
	return *this / (*this)[3];
}


Float3D Point2D::Length()const
{
	return sqrt( At(0,0)*At(0,0) + At(1,0)*At(1,0) + At(2,0)*At(2,0) );
}

Point2D Point2D::Normalize()const
{
	assert( !is_zero((*this)[3]) );
	return *this / (*this)[3];
}

Float3D KahanSum(const Float3D A[], int size)
{
	//KahanSum
	Float3D sum=0, C=0, Y, T;

	for (int i=0; i<size; i++)
	{
		Y = A[i] - C;
		T = sum + Y;
		C = T - sum - Y;
		sum = T;
	}
	return sum;
}


